#include <mc_path_planning/path_planning_base.h>

PathPlanningBase::PathPlanningBase() {
    startPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/start_pose", 1, true);
    goalPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/goal_pose", 1, true);
    pathPub_ = nh_.advertise<nav_msgs::Path>("/global_path", 1, true);
    pathOriginPub_ = nh_.advertise<nav_msgs::Path>("/global_path_origin", 1, true);
}

PathPlanningBase::~PathPlanningBase() {

}


// 路径规划
bool PathPlanningBase::PathPlanning(Pose2D start, Pose2D goal, vector<Pose2D>& points){
    bool status = false;
    ros::Time startTime = ros::Time::now();

    // 路径点坐标转换
    Vec2i start_point = WorldToOpencvPixel(msg_convert::ToVec2f(start));
    Vec2i goal_point = WorldToOpencvPixel(msg_convert::ToVec2f(goal));

    // 开始全局路径规划
    vector<Vec2i> global_points;
    vector<Vec2f> global_points_2f;
    LOG(INFO) << "[Planning | Meter] start: " << start.transpose() << ", goal: " << goal.transpose();
    LOG(INFO) << "[Planning | Pixel] start: " << start_point << ", goal: " << goal_point;
    status = PathGenerate(start_point, goal_point, global_points);
    if(status){
        // 坐标转换
        for(auto point : global_points){
            global_points_2f.emplace_back(OpencvPixelToWorld(point));
        }

        // 发布原始路径
        nav_msgs::Path path_origin_msg = Points2Path(global_points_2f, "map");
        pathOriginPub_.publish(path_origin_msg);

        // 路径平滑处理
        // PathSmootherFivePoint(global_points_2f, global_points_2f);
        PathSmootherBezierCurve3(global_points_2f, global_points_2f);
    }

    //格式转换
    points.clear();
    for(auto point : global_points_2f){
        points.emplace_back(msg_convert::ToPose2D(point));
    }
    points = GenPathDirection(points, goal);    //生成方向数据

    // debug
    // if(points.size() > 0){
    //     cout << "--> front point: " << points.front().transpose() << endl;
    //     cout << "--> back point: " << points.back().transpose() << endl;
    // }

    // 发布路径
    nav_msgs::Path path_msg = msg_convert::ToPath(points, "map");
    pathPub_.publish(path_msg);

    double timeInterval = (ros::Time::now() - startTime).toSec();
    LOG(INFO) << "[PathPlanning] planning end. points size: " << points.size()
        << ", time_interval: "  << timeInterval;   
    return status;
}





// 点集转路径
nav_msgs::Path PathPlanningBase::Points2Path(vector<Vec2f>& points, string frame_id){
    nav_msgs::Path path_msg;
    path_msg.header.frame_id = frame_id;
    path_msg.header.stamp = ros::Time::now();
    for(auto point : points){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.pose.position.x = point.x;
        pose_stamp.pose.position.y = point.y;
        path_msg.poses.emplace_back(pose_stamp);
    } 
    return path_msg;   
}

// 路径方向计算
vector<Pose2D> PathPlanningBase::GenPathDirection(vector<Pose2D>& points, Pose2D goal){
    vector<Pose2D> new_points;
    if(points.size() <= 2){
        new_points = points;
        return new_points;
    }

    for(int i=0; i<points.size()-1; i++){
        Pose2D curr_point = points[i];
        Pose2D next_point = points[i+1];
        curr_point[2]= math_utils::AnglePose2D(curr_point, next_point);
        new_points.emplace_back(curr_point);
    }
    new_points.emplace_back(goal);
    return new_points;
}